/*
* @Date: 2020-10-04 19:56:32
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-07 01:30:10
 * @FilePath: /swarm_ws/src/px4_swarm_control/src/UWBRosNode.cpp
 * @Description: UWBros node 管理类
 * 使用方法：在/send_topic 中发布send_buffer内容data内容即发布的内容
 * 发布incomingbrocastdata 包含了id信息，故发送时不用关心自己id
*/
#include "UWBRosNode.h"

UWBRosNode* UWBRosNode::getInstance(){
    if(instance == nullptr)instance = new UWBRosNode();
    return instance;
}

void UWBRosNode::setUWBRosNode(std::string serial_name, int baudrate, ros::NodeHandle nh, bool enable_debug)
{
    UWBManager::setUWBManager(serial_name, baudrate, enable_debug);
    nh.param<int>("send_buffer", send_buffer_size, 80);
    nh.param<int>("id", self_id, -1);
    nh.param<double>("send_freq", send_freq, 50);
    nh.param<double>("recv_freq", recv_freq, 100);
    nh.param<bool>("enable_uwb_send", enable_uwb_send, true);
    nh.param<std::string>("send_topic_name", send_topic_name, "/send_topic");

    remote_node_pub = nh.advertise<remote_uwb_info>("/swarm_state/remote_nodes", 1);
    broadcast_data_pub = nh.advertise<incoming_broadcast_data>("/incoming_broadcast_data", 1);
    recv_bdmsg = nh.subscribe(send_topic_name, 1, &UWBRosNode::onSendBroadcastReq, this, ros::TransportHints().tcpNoDelay());
    fast_timer = nh.createTimer(ros::Duration(1/recv_freq), &UWBRosNode::readAndParseCallback, this);
    slow_timer = nh.createTimer(ros::Duration(1/send_freq), &UWBRosNode::sendBroadcastDataCallback, this);
    time_reference_pub = nh.advertise<sensor_msgs::TimeReference>("time_ref", 1);
    watchSignal();
}


void 
UWBRosNode::sendBroadcastDataCallback(const ros::TimerEvent &e) {
    if (uwb_ok) {
        send_lock.lock();
        ROS_INFO_THROTTLE(1.0, "UWb Send buffer %ld", send_buffer.size());
        if (send_buffer.size() > u_int (2 * send_buffer_size)) {
            ROS_WARN("Send buffer size %ld too big!", send_buffer.size());
        }
        if (send_buffer.size() <= u_int(send_buffer_size)) {
            if (!send_buffer.empty()) {
                this->sendBroadcastData(send_buffer);
                send_buffer.clear();
            }
        } else {
            std::vector<uint8_t> sub(&send_buffer[0], &send_buffer[send_buffer_size]);
            this->sendBroadcastData(sub);
            send_buffer.erase(send_buffer.begin(), send_buffer.begin() + send_buffer_size);
        }
        send_lock.unlock();
    }
}

void 
UWBRosNode::readAndParseCallback(const ros::TimerEvent &e) {
    this->readAndParse();
}

void 
UWBRosNode::onSendBroadcastReq(data_buffer msg){
    if ((msg.send_method == 0 || msg.send_method == 2)&&(enable_uwb_send)) {
        send_lock.lock();
        send_buffer.insert(send_buffer.end(), msg.data.begin(), msg.data.end());
        send_lock.unlock();
    }
}


void 
UWBRosNode::onBroadcastDataRecv(int _id, Buffer _msg){
    UWBManager::onBroadcastDataRecv(_id, _msg);
    recv_lock.lock();
    if(enable_parse_buffer)
        recv_data_buffer.insert(recv_data_buffer.end(),_msg.begin(),_msg.end());
    recv_lock.unlock();
    incoming_broadcast_data data;
    data.header.stamp = ros::Time::now();
    data.lps_time = sys_time;
    data.remote_id = _id;
    data.data = _msg;
    broadcast_data_pub.publish(data);
}

void
UWBRosNode::parseBroadcastDataRecv(){
    recv_lock.lock();
    if(!recv_data_buffer.empty()&&enable_parse_buffer){
        
    }
    recv_lock.unlock();
}

void 
UWBRosNode::onSystemTimeUpdate(){
    sensor_msgs::TimeReference tr;
    tr.header.stamp = ros::Time::now();
    tr.source = "LPS";
    tr.time_ref = ros::Time(0) + ros::Duration(sys_time/1000.0);

    time_reference_pub.publish(tr);
}

void 
UWBRosNode::onNodeDataUpdated(){
    UWBManager::onNodeDataUpdated();
    remote_uwb_info info;
    info.sys_time = this->sys_time;
    info.remote_node_num = this->nodes_info.size();
    info.self_id = this->self_id;
    info.header.stamp = ros::Time::now();
    for (auto k : this->nodes_info) {
        int _id = k.first;
        if (_id < MAX_NUM) {
                RemoteNodeInfo nod = k.second;
                info.node_ids.push_back(_id);
                info.node_dis.push_back(nod.distance);
                info.recv_distance_time.push_back(nod.dis_time);
                info.active.push_back(nod.active);
                info.fp_rssi.push_back(nod.fp_rssi);
                info.rx_rssi.push_back(nod.rx_rssi);
        }
    }
    remote_node_pub.publish(info);
    ROS_INFO_THROTTLE(1,"[timestamp %d] ID %d nodes total %d active %d\n",
            sys_time, self_id, info.remote_node_num, valid_node_quantity);
}



void
UWBRosNode::shutdown_handler(int sig) {
    printf("Shutting down uwb .....\n");
    instance->close_port();
    exit(0);
}

void
UWBRosNode::watchSignal(){
    signal(SIGINT, UWBRosNode::shutdown_handler);
    signal(SIGTERM, UWBRosNode::shutdown_handler);
    signal(SIGKILL, UWBRosNode::shutdown_handler);
    signal(SIGQUIT, UWBRosNode::shutdown_handler);
}

UWBRosNode *UWBRosNode::instance = nullptr;

